#include "task_commiut.h"

#include "FreeRTOS.h"
#include "task.h"
#include <stdio.h>
#include "main.h"
#include "usart.h"

#include "mpu9250.h"
#include "runge_kutta.h"

#define TASK_PERIOD pdMS_TO_TICKS(10) // 设置周期为1ms


struct vofa_data_t
{
    float data[14];
    const uint8_t tail[4];
}__packed vofa_buffer =
    {
        .tail = {0x00, 0x00, 0x80, 0x7f},
};

void Commiut_Task(void const *argument)
{
    TickType_t xLastWakeTime;

    // 初始化上次唤醒时间为当前时间
    xLastWakeTime = xTaskGetTickCount();

    HAL_UART_Transmit(&huart1, (uint8_t *)"Commiut Task builded!\n", 25, 1000);

    for (;;)
    {
        

 
float roll, pitch, yaw;
 
    // roll (x-axis rotation)
    float sinr_cosp = 2 * (imu.q.w * imu.q.x + imu.q.y * imu.q.z);
    float cosr_cosp = 1 - 2 * (imu.q.x * imu.q.x + imu.q.y * imu.q.y);
    roll = atan2(sinr_cosp, cosr_cosp);
 
    // pitch (y-axis rotation)
    double sinp = 2 * (imu.q.w *imu. q.y - imu.q.z * imu.q.x);
    if (abs(sinp) >= 1)
        pitch = copysign(3.141592 / 2, sinp); // use 90 degrees if out of range
    else
        pitch = asin(sinp);
 
    // yaw (z-axis rotation)
    float siny_cosp = 2 * (imu.q.w * imu.q.z + imu.q.x * imu.q.y);
    float cosy_cosp = 1 - 2 * (imu.q.y * imu.q.y + imu.q.z * imu.q.z);
    yaw = atan2(siny_cosp, cosy_cosp);
 


    vofa_buffer.data[0] = mpu9250.mpu_data.Accel[0];
    vofa_buffer.data[1] = mpu9250.mpu_data.Accel[1];
    vofa_buffer.data[2] = mpu9250.mpu_data.Accel[2];
    vofa_buffer.data[3] = mpu9250.mpu_data.Gyro[0];
    vofa_buffer.data[4] = mpu9250.mpu_data.Gyro[1];
    vofa_buffer.data[5] = mpu9250.mpu_data.Gyro[2];
    vofa_buffer.data[6] = imu.q.w;
    vofa_buffer.data[7] = imu.q.x;
    vofa_buffer.data[8] = imu.q.y;
    vofa_buffer.data[9] = imu.q.z;
    vofa_buffer.data[10] = mpu9250.mpu_data.Temp;

    vofa_buffer.data[11] = roll;
    vofa_buffer.data[12] = pitch;
    vofa_buffer.data[13] = yaw;
    HAL_UART_Transmit_DMA (&huart1,(uint8_t *)&vofa_buffer,sizeof(vofa_buffer));

    vTaskDelayUntil(&xLastWakeTime, TASK_PERIOD);

    }
}